/**
 * 头文件引入
 */
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <cmath>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Image.h>
#include <laser_test/laser_control.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include "opencv2/opencv.hpp"
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl_ros/point_cloud.h>

/**
 * 目前雷达使用的功能是：
 * 1.避障前测距，如果距离小于0.5m，则向主节点发布消息，主节点收到消息后，停止运动
 * 2.倒车入库定位，加距离停车
 * 3.正车入库定位，加距离停车
 * 4.侧方停车识别和定位，加距离停车
 */
/**
 * 命名空间使用
 */
using namespace std;
using namespace cv;

//电机极性定义，此处为-1代表speed小于0前进，大于零后退
#define dianji_fuhao -1
/**
 * 命名空间定义，方便后续编写者使用，防止重名,在这里定义全局变量
 */
namespace zeal
{



//使用的消息类型定义
ackermann_msgs::AckermannDriveStamped  ackermann_cmd;//雷达控制小车底盘的消息
sensor_msgs::LaserScan  zeal_laser; // 雷达数据的副本，用于后续函数值传递，可以优化成常引用传递
std_msgs::Int32 call_back_data; //雷达向主节点发布的消息
																/*
																call_back_data雷达向主节点发送识别结果
																1.call_back_data.data = 1;表示识别到障碍物,请求主节点发送执行避障指令
																2.call_back_data.data = 2;表示结束避障
                                3.call_back_data.data = 3;表示倒车入库完成
                                4.call_back_data.data = 4;表示正车入库完成
                                5.call_back_data.data = 5;表示识别到要进行侧方停车
																6.call_back_data.data = 6;表示侧方停车完成

																*/

//使用的全局变量定义
int park_cmdflag = 0; //主节点发过来的命令

//避障的全局变量
int bizhang_control_flag = 0;

//倒车入库全局变量
int daocheruku_flag = 1;

//正车入库全局变量
int zhengcheruku_flag = 1;

//侧方停车全局变量
int cefang_parking_first_flag = 1;
int cefang_parking_second_flag = 1;
int saomiao_open_flag = 1;
int jiaodu_jiaozhun_count = 0;
int right_angle_flag = 2;
/**********雷达不同模式控制函数声明 */

int bizhang_shibie();

int bizhang_control();


/**********自定义子函数声明 */
/**
 * @name fanwei_nearest_dist_daixianzhi
 * 功能：在指定范围内，寻找最近距离，并返回距离多少米
 * @param dist_xianzhi 距离限制，单位：m，在一个范围内，如果距离小于这个值，则不参与计算
 * @param qidian 起始点,这个值对应1440个扫描点中的某个点，例如0-90°，那就是qidian = 0*4 = 0
 * @param zhongdian 终止点，这个值对应1440个扫描点中的某个点，zhongdian = 90*4 = 360
 * @return 返回最近距离，单位：m
 */
float fanwei_nearest_dist_daixianzhi(float dist_xianzhi,int qidian,int zhongdian);//按0-360，从0-1440个点

/**
 * @name zeak_get_my_nearest_angle
 *功能：在指定范围内，寻找最近距离，并返回弧度
 * @param zuo_bianjie 左边界，单位：度
 * @param you_bianjie 右边界，单位：度
 * @param juli_xianzhi 距离限制，单位：m，在一个范围内，如果距离小于这个值，则不参与计算
 * @return 返回当前范围内最近距离的弧度，单位：弧度 范围是0~2*PI
 * 注意：这个函数的返回值是弧度，是弧度，是弧度！！！。添加的距离限制是为了防止车本身距离的干扰
 */
float zeal_get_my_nearest_angle_julixianzhi(int zuo_bianjie, int you_bianjie,float juli_xianzhi);


/**
 * @name zeal_cefangruku_first_get_angle
 * @param zuo_qishi 左起始点，单位：度
 * @param you_qishi 右起始点，单位：度
 * @param juli_xianzhi 距离限制，单位：m，在一个范围内，如果距离小于这个值，则不参与计算
 * @param zuo_bianjie 左边界，用于当没找到中心角度时的补偿角度，单位：度
 * @param you_bianjie 右边界，用于当没找到中心角度时的补偿角度，单位：度
 * @return 返回当前范围内目标的中心角度，单位：角度 0~360
 */
int zeal_cefangruku_L2R_angle(int zuo_qishi,int you_qishi,float juli_xianzhi,int zuo_bianjie,int you_bianjie);




/**
 * @name front_adjust
 * 功能：前向入库，如果距离小于0.5m，则向主节点发布消息，主节点收到消息后，停止运动
 * @param dist_xianzhi 距离限制，单位：m，在一个范围内，如果距离小于这个值，则不参与计算
 * @param qidian 起始点,这个值对应1440个扫描点中的某个点，例如0-90°，那就是qidian = 0*4 = 0
 * @param zhongdian 终止点，这个值对应1440个扫描点中的某个点，zhongdian = 90*4 = 360
 * @param zuo_bianjie 左边界，单位：度
 * @param you_bianjie 右边界，单位：度
 * @param speed 速度，单位：m/s
 * @param set_angle 目标角度，单位：度
 * @return 返回当前范围内最近距离,单位：m
 */
float front_adjust(float dist_xianzhi,int qidian,int zhongdian,int zuo_bianjie,int you_bianjie,int speed,float set_angle);

/**
 * @name back_adjust
 * 功能：后向入库，如果距离小于0.5m，则向主节点发布消息，主节点收到消息后，停止运动
 * @param dist_xianzhi 距离限制，单位：m，在一个范围内，如果距离小于这个值，则不参与计算
 * @param qidian 起始点,这个值对应1440个扫描点中的某个点，例如0-90°，那就是qidian = 0*4 = 0
 * @param zhongdian 终止点，这个值对应1440个扫描点中的某个点，zhongdian = 90*4 = 360
 * @param zuo_bianjie 左边界，单位：度
 * @param you_bianjie 右边界，单位：度
 * @param speed 速度，单位：m/s
 * @param set_angle 目标角度，单位：度
 * @return 返回当前范围内最近距离,单位：m
 */
float back_adjust(float dist_xianzhi,int qidian,int zhongdian,int zuo_bianjie,int you_bianjie,int speed,float set_angle);

//ROS节点发布者
ros::Publisher pub_vel; //向小车底盘发送转角和速度信息
ros::Publisher pub_call_back; //向主节点发送消息
ros::Duration duration1(3);//延时器对象声明
}


void Park_callback(const std_msgs::Int32::ConstPtr& msg) // 接收主节点发过来的命令
{
  zeal::park_cmdflag = msg->data; //park_cmdflag里面的值由主节点发过来，很重要！
																	//因为他直接决定雷达内部的执行内容！
																	/*
																	1.park_cmdflag = 1 代表避障识别
																	2.park_cmdflag = 2 代表执行避障
																	3.park_cmdflag = 3 代表执行倒车入库
																	4.park_cmdflag = 4 代表执行正车入库
																	5.park_cmdflag = 5 代表识别侧方停车
																	6.park_cmdflag = 6 代表执行侧方停车
																	*/
}



/**
 * @name 雷达数据回调函数
 * 功能：每获取到一次雷达数据，就会执行一次，类似中断函数，最主要的处理函数
 * @param laser 雷达数据
 *
 */
void zeal_laser_callback(const sensor_msgs::LaserScan::ConstPtr& laser)
{
  zeal::zeal_laser=*laser; //拿到雷达数据，重映射到全局变量中，方便后续函数使用

	//下面开始雷达模式的选择
	switch(zeal::park_cmdflag)
	{
		case 1: //避障识别模式
		{
			int bizhang_judge_flag = 0;
			ROS_INFO_STREAM("zeal::park_cmdflag = 1"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 1"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 1"<<endl);
			bizhang_judge_flag = zeal::bizhang_shibie();
			if(bizhang_judge_flag)
			{

				zeal::call_back_data.data = 1;//表示识别到障碍物,请求主节点发送执行避障指令
				zeal::pub_call_back.publish(zeal::call_back_data);
			}
			break;
		}
		case 2: //执行避障模式
		{
			int bizhang_control_flag = 0;
			ROS_INFO_STREAM("zeal::park_cmdflag = 2"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 2"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 2"<<endl);
			bizhang_control_flag = zeal::bizhang_control();
			if(bizhang_control_flag)
			{
				zeal::call_back_data.data = 2;//表示避障完成，请求主节点发送停止指令
				zeal::pub_call_back.publish(zeal::call_back_data);
			}
			break;
		}
		case 3: //执行倒车入库模式
		{
			ROS_INFO_STREAM("zeal::park_cmdflag = 3"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 3"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 3"<<endl);
      if(zeal::daocheruku_flag == 1)
      {
        float daoche_distance = zeal::back_adjust(0.2,1000,1160,250,290,20,270);
        if(daoche_distance < 1.2)
        {
          zeal::daocheruku_flag = 2;
        }
      }
      else if(zeal::daocheruku_flag == 2)
      {
        float daoche_distance = zeal::back_adjust(0.2,800,1360,250,290,15,270);
        if(daoche_distance < 0.37)
        {
          zeal::daocheruku_flag = 3;
          zeal::ackermann_cmd.drive.speed = 0;
          zeal::ackermann_cmd.drive.steering_angle = 0;
          zeal::pub_vel.publish(zeal::ackermann_cmd);
          zeal::call_back_data.data = 3;//表示倒车入库完成，请求主节点发送停止指令
          zeal::pub_call_back.publish(zeal::call_back_data);
        }
      }
			break;
		}
		case 4: //执行正车入库模式
		{
			ROS_INFO_STREAM("zeal::park_cmdflag = 4"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 4"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 4"<<endl);
      if(zeal::zhengcheruku_flag == 1)
      {
        float zhengche_distance = zeal::front_adjust(0.2,240,480,60,120,-20,80);
        if(zhengche_distance < 1.4)
        {
          zeal::zhengcheruku_flag = 2;
        }
      }
      else if(zeal::zhengcheruku_flag == 2)
      {
        float zhengche_distance = zeal::front_adjust(0.2,220,500,55,125,-15,85);
        if(zhengche_distance < 0.3)
        {
          zeal::zhengcheruku_flag = 3;
          zeal::ackermann_cmd.drive.speed = 0;
          zeal::ackermann_cmd.drive.steering_angle = 0;
          zeal::pub_vel.publish(zeal::ackermann_cmd);
          zeal::call_back_data.data = 4;//表示正车入库完成，请求主节点发送停止指令
          zeal::pub_call_back.publish(zeal::call_back_data);
        }
      }
      break;
		}
		case 5: //识别侧方停车模式
		{
			ROS_INFO_STREAM("zeal::park_cmdflag = 5"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 5"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 5"<<endl);
      float cefang_distance = zeal::fanwei_nearest_dist_daixianzhi(0.2,140,360);//按0-360，从0-1440个点
      if(cefang_distance < 0.7)
      {
        zeal::ackermann_cmd.drive.speed = 0;
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::call_back_data.data = 5;//表示识别到侧方停车,请求主节点发送执行侧方停车指令
        zeal::pub_call_back.publish(zeal::call_back_data);
      }
      break;
		}
		case 6: //执行侧方停车模式,一阶段
		{
      //首先是执行一套动作组，然后不停调整位置，直到车子大致进入停车区，然后交给主节点进行调整，调整结束后，再交给雷达节点
      //最后进行停车格子的选择。停下后，告诉主节点侧方停车结束，转到stop模式

			ROS_INFO_STREAM("zeal::park_cmdflag = 6"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 6"<<endl);
			ROS_INFO_STREAM("zeal::park_cmdflag = 6"<<endl);
      if(zeal::cefang_parking_first_flag == 1)
      {
        zeal::ackermann_cmd.drive.steering_angle = -50;
        zeal::ackermann_cmd.drive.speed = -20*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(4.5);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::ackermann_cmd.drive.steering_angle = 50;
        zeal::ackermann_cmd.drive.speed = 20*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(4);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = 20*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(4.5);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = 0*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::cefang_parking_first_flag =2 ;

      }
      else if(zeal::cefang_parking_first_flag == 2)
      {
          if(zeal::saomiao_open_flag == 1)
        {
        // float jiaodu = zeal::zeal_get_my_nearest_angle_julixianzhi(0, 110 , 0);//左右边界单位是°
        // int jiaodu_jiaodu = jiaodu/CV_PI*180;
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = 0;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        int jiaodu_jiaodu = zeal::zeal_cefangruku_L2R_angle(100 , 0 , 0.1 , 0 , 110);
        int jiaodu_xiuzheng_90_error_ = 90-jiaodu_jiaodu;
        //进行绝对值判断
        int jiaodu_xiuzheng_abs = abs(jiaodu_xiuzheng_90_error_);
        if (jiaodu_xiuzheng_abs <= 20)
        {
          std::cout << "enter_next_cefang_flag________________!!!!" << std::endl;
          zeal::cefang_parking_first_flag = 3;
        }
        zeal::jiaodu_jiaozhun_count = jiaodu_xiuzheng_90_error_/20;
        ROS_INFO_STREAM("xiuzhengjiaodu  "<< jiaodu_xiuzheng_90_error_ <<endl);
        ROS_INFO_STREAM("cishu "<<zeal::jiaodu_jiaozhun_count <<endl);

        //扫描结束，标志清零
        zeal::saomiao_open_flag = 0;
        }

        else
        {
          if(zeal::jiaodu_jiaozhun_count > 0) zeal::right_angle_flag = 1;
          else if(zeal::jiaodu_jiaozhun_count < 0) zeal::right_angle_flag = 0;

          //出错的情况，直接报错
          else if(zeal::jiaodu_jiaozhun_count == 0)
          {
            zeal::right_angle_flag = -1;
            std::cout << "cefang_count is zero" << std::endl;
          }


        //校正次数取绝对值
        zeal::jiaodu_jiaozhun_count = abs(zeal::jiaodu_jiaozhun_count);

		//将校准次数强制置位为1
		zeal::jiaodu_jiaozhun_count = 1;


          while(zeal::jiaodu_jiaozhun_count)
          {
            zeal::jiaodu_jiaozhun_count--;
            if(zeal::right_angle_flag == 1)
            {
              zeal::ackermann_cmd.drive.steering_angle = 50;
              zeal::ackermann_cmd.drive.speed = -15*dianji_fuhao;
              zeal::pub_vel.publish(zeal::ackermann_cmd);
              zeal::duration1 = ros::Duration(1);//延迟3秒
              zeal::duration1.sleep();//调用延迟函数


              zeal::ackermann_cmd.drive.steering_angle = -50;
              zeal::ackermann_cmd.drive.speed = 15*dianji_fuhao;
              zeal::pub_vel.publish(zeal::ackermann_cmd);
              zeal::duration1 = ros::Duration(1);//延迟3秒
              zeal::duration1.sleep();//调用延迟函数
            }
            else if(zeal::right_angle_flag == 0)
            {
              zeal::ackermann_cmd.drive.steering_angle = -50;
              zeal::ackermann_cmd.drive.speed = -15*dianji_fuhao;
              zeal::pub_vel.publish(zeal::ackermann_cmd);
              zeal::duration1 = ros::Duration(1);//延迟3秒
              zeal::duration1.sleep();//调用延迟函数


              zeal::ackermann_cmd.drive.steering_angle = 50;
              zeal::ackermann_cmd.drive.speed = 15*dianji_fuhao;
              zeal::pub_vel.publish(zeal::ackermann_cmd);
              zeal::duration1 = ros::Duration(1);//延迟3秒
              zeal::duration1.sleep();//调用延迟函数
            }

          }
          // 重新扫描
          zeal::saomiao_open_flag = 1;

        }
        }
      else if(zeal::cefang_parking_first_flag == 3)
      {
        float cefang_parking_first_distance = zeal::front_adjust(0,240,480,60,120,-15,80);
        if(cefang_parking_first_distance < 0.35)
        {
          zeal::cefang_parking_first_flag = 4;
          zeal::ackermann_cmd.drive.speed = 0;
          zeal::ackermann_cmd.drive.steering_angle = 0;
          zeal::pub_vel.publish(zeal::ackermann_cmd);
          zeal::call_back_data.data = 6;//表示侧方停车第一阶段结束，请求主节点在主节点里执行校准，并等待主节点发回来的第二阶段开始的指令
          zeal::pub_call_back.publish(zeal::call_back_data);
        }
      }
      break;
		}
    case 7: //执行侧方停车模式,二阶段
    {
      ROS_INFO_STREAM("zeal::park_cmdflag = 7"<<endl);
      ROS_INFO_STREAM("zeal::park_cmdflag = 7"<<endl);
      ROS_INFO_STREAM("zeal::park_cmdflag = 7"<<endl);
      if(zeal::cefang_parking_second_flag == 1)
      {
        float zuihou_jiaodu = zeal::zeal_cefangruku_L2R_angle(120 , 60, 0.1 , 20 , 140);
        float cefang_parking_second_distance = zeal::fanwei_nearest_dist_daixianzhi(0.1,360,440);
        zeal::ackermann_cmd.drive.speed = 15*dianji_fuhao;
        zeal::ackermann_cmd.drive.steering_angle = (zuihou_jiaodu-90)*2.5;
        zeal::pub_vel.publish(zeal::ackermann_cmd);   
        if(cefang_parking_second_distance > 1.15)  //这个距离需要调参，代表停到对应的格子里
        {
          zeal::cefang_parking_second_flag = 2;
          zeal::ackermann_cmd.drive.speed = 0;
          zeal::ackermann_cmd.drive.steering_angle = 0;
          zeal::pub_vel.publish(zeal::ackermann_cmd);
      }
      else if(zeal::cefang_parking_second_flag == 2)
      {
        zeal::call_back_data.data = 7;//表示侧方停车，按模块定位全部结束，告诉主节点关闭一切控制
        zeal::pub_call_back.publish(zeal::call_back_data);
      }
      break;
    }
		default:
		{
			ROS_INFO_STREAM("kongxian"<<endl);
			ROS_INFO_STREAM("kongxian"<<endl);
			ROS_INFO_STREAM("kongxian"<<endl);
      break;
		}

	}
}
}
////////////////////////雷达回调结束




/**
 * 具体模式的主控制函数定义
 */
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int zeal::bizhang_shibie()
{
	float now_distance = zeal::fanwei_nearest_dist_daixianzhi(0.2,270,450);//按0-360，从0-1440个点
	if(now_distance < 0.45)
	{
		ROS_INFO_STREAM("The obstacle is detected!!!!!!!!!"<<endl);
		return 1;
	}
	else
	{
		return 0;
	}
	/*
	明天再写！
	*/

}

int zeal::bizhang_control() // 写死的避障模式
{
	 if(zeal::bizhang_control_flag == 0 )
    { //1
        ROS_INFO_STREAM("turn left"<<endl);
        zeal::ackermann_cmd.drive.steering_angle = -70;//左转弯  角度需要调参
        zeal::ackermann_cmd.drive.speed = -20*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(4);//延迟4秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::bizhang_control_flag = 1;
		return 0;
    }

    else if (zeal::bizhang_control_flag == 1)
    {
        ROS_INFO_STREAM("straight1111"<<endl);
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = -25*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(1.5);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::bizhang_control_flag = 2;
		return 0;
    }

    else if(zeal::bizhang_control_flag == 2)
    {
        ROS_INFO_STREAM("turn right"<<endl);
        zeal::ackermann_cmd.drive.steering_angle = 70;//右转弯  角度需要调参
        zeal::ackermann_cmd.drive.speed = -20*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(6);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::bizhang_control_flag = 3;
		return 0;
    }

    else if(zeal::bizhang_control_flag == 3)
    {
        ROS_INFO_STREAM("straight2222"<<endl);
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = -25*dianji_fuhao;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::duration1 = ros::Duration(1.5);//延迟3秒
        zeal::duration1.sleep();//调用延迟函数
        zeal::bizhang_control_flag = 4;
		return 0;
    }

    else if(zeal::bizhang_control_flag == 4)
    {
        ROS_INFO_STREAM("stop"<<endl);
        zeal::ackermann_cmd.drive.steering_angle = 0;
        zeal::ackermann_cmd.drive.speed = 0;
        zeal::pub_vel.publish(zeal::ackermann_cmd);
        zeal::call_back_data.data = 2; // 为2代表避障结束，向主节点发送信息
        zeal::pub_call_back.publish(zeal::call_back_data);
        zeal::bizhang_control_flag = 5; //标志位，代表避障结束
		return 1;
    }

}




/**
 * 雷达所用到所有自定义功能实现子函数的定义
 */
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float zeal::fanwei_nearest_dist_daixianzhi(float dist_xianzhi,int qidian,int zhongdian)//按0-360，从0-1440个点
{
  float dist_array[zhongdian-qidian+1]={0};
  float now_distance = 0;
  float tihuan_distance = 200;

  //这里开始打印所有扫描点的角度和距离信息
  for(int i=0;i<zhongdian-qidian+1;i++)
  {
    dist_array[i] = zeal::zeal_laser.ranges[i+qidian];
    if(dist_array[i]>5000)
    {
    }
    else if(dist_array[i]<tihuan_distance && dist_array[i]>dist_xianzhi)
    {
      tihuan_distance = dist_array[i];
    }
  }
  if(tihuan_distance >= 100) tihuan_distance = 5000;//如果tihuan_distance大于100，则代表没有检测到障碍物
  now_distance = tihuan_distance;
  ROS_INFO_STREAM("now_distance "<<now_distance<<endl);
  return now_distance;
}


float zeal::zeal_get_my_nearest_angle_julixianzhi(int zuo_bianjie, int you_bianjie,float juli_xianzhi)//左右边界单位是°
{
 int l = 4*zuo_bianjie;
 int r = 4*you_bianjie;
 int tt=0;
 float my_nearest_angle=0;
 float my_nearest_dist=100;
 for(int i=l;i<r;i=i+4)
    {
    if (zeal::zeal_laser.ranges[i]<my_nearest_dist && zeal::zeal_laser.ranges[i]>juli_xianzhi)
    {
      tt++;
      if (tt==1)
      {
        my_nearest_dist=zeal::zeal_laser.ranges[i];
        my_nearest_angle =(zeal::zeal_laser.angle_min+i*zeal::zeal_laser.angle_increment);
      }
    }
    else
      tt=0;
    }
return my_nearest_angle; //弧度
}

int zeal::zeal_cefangruku_L2R_angle(int zuo_qishi,int you_qishi,float juli_xianzhi,int zuo_bianjie,int you_bianjie)//左右边界单位是°
{
 int l = 4*zuo_qishi;
 int r = 4*you_qishi;
 static float last_distance = 0;
 static bool gengxin_flag = true;
 static bool left_bianjie_flag = true;
 static bool right_bianjie_flag = false;
 bool get_answer = false;
 static int  left_bianjie = 0;
 static int  right_biajie = 0;
 float now_distance = 0;
 float distance_chazhi = 0;
 for(int i = l+4;i>r;i = i-4)//从左到右扫描，遍历每一个点
 {
    //不断获取当前距离信息
    now_distance = zeal::zeal_laser.ranges[i];
    //更新第一次上一次距离信息
    if(gengxin_flag)
    {
      last_distance = zeal::zeal_laser.ranges[i];
      gengxin_flag = false;
    }
    //计算上一次和这一次的距离差值
    distance_chazhi = abs(now_distance - last_distance);
    //更新左右边界信息
    if(distance_chazhi > 0.6 && left_bianjie_flag && now_distance > juli_xianzhi && now_distance < 1.2)
    {
      left_bianjie = (zeal::zeal_laser.angle_min+i*zeal::zeal_laser.angle_increment)/CV_PI*180;
      right_bianjie_flag = true;
      left_bianjie_flag = false;
    }
    //更新右边界信息
    else if(distance_chazhi > 0.6 && right_bianjie_flag && now_distance > juli_xianzhi && now_distance < 1.2)
    {
      right_biajie = (zeal::zeal_laser.angle_min+i*zeal::zeal_laser.angle_increment)/CV_PI*180;
      left_bianjie_flag = true;
      right_bianjie_flag = false;
      gengxin_flag = true;

      int mid_angle = (left_bianjie+right_biajie) >> 1;
      left_bianjie = 0;
      right_biajie = 0;
      get_answer = true; //代表找到了左右边界，并确定了中心的角度
      std::cout<<"mid_angle="<<mid_angle<<std::endl;
      return mid_angle;
    }
    //更新上一次距离信息
    if(!gengxin_flag)
    {
      last_distance = now_distance;
    }

 }
 //增强代码健壮性，如果for循环里没找到中心角度，就使用使用之前获取角度的办法，进行补救
 if(!get_answer)
 {
   float bujiu_hudu = zeal::zeal_get_my_nearest_angle_julixianzhi(zuo_bianjie,you_bianjie,juli_xianzhi);
   return (int)(bujiu_hudu/CV_PI*180);
 }

}


float zeal::front_adjust(float dist_xianzhi,int qidian,int zhongdian,int zuo_bianjie,int you_bianjie,int speed,float set_angle)
{
    float now_distance = zeal::fanwei_nearest_dist_daixianzhi(dist_xianzhi,qidian,zhongdian);
    float now_angle = zeal::zeal_get_my_nearest_angle_julixianzhi(zuo_bianjie,you_bianjie,dist_xianzhi);
    float angle_error = now_angle/CV_PI*180 - set_angle;

    zeal::ackermann_cmd.drive.steering_angle = -angle_error*2.5; //这里的2.5是一个参数，可以理解为kp，需要调试，越大，超调量越大，角度变化幅度越大
    zeal::ackermann_cmd.drive.speed = speed * dianji_fuhao;
    zeal::pub_vel.publish(zeal::ackermann_cmd);
    return now_distance;
}



float zeal::back_adjust(float dist_xianzhi,int qidian,int zhongdian,int zuo_bianjie,int you_bianjie,int speed,float set_angle)
{
    float now_distance = zeal::fanwei_nearest_dist_daixianzhi(dist_xianzhi,qidian,zhongdian);
    float now_angle = zeal::zeal_get_my_nearest_angle_julixianzhi(zuo_bianjie,you_bianjie,dist_xianzhi);
    float angle_error = now_angle/CV_PI*180 - set_angle;

    zeal::ackermann_cmd.drive.steering_angle = angle_error*2.5;
    zeal::ackermann_cmd.drive.speed = speed * dianji_fuhao;
    zeal::pub_vel.publish(zeal::ackermann_cmd);
    return now_distance;

}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////





////////////////////////////////////主函数
int main(int argc,char** argv){
  ros::init(argc,argv,"zeal_laser");
  ros::NodeHandle nh;

  ros::Subscriber sublaser=nh.subscribe("/scan",1,&zeal_laser_callback);//比赛中实际使用的回调函数
  ros::Subscriber subpark=nh.subscribe("/park_cmd",1,&Park_callback);//由主节点发送过来
  //park_cmdflag=9时，为识别避障，10为开始执行避障；park_cmdflag=2时，为正车入库；park_cmdflag=3时，为侧方停车
  zeal::pub_vel=nh.advertise<ackermann_msgs::AckermannDriveStamped>("/ackermann_cmd",1);
  zeal::pub_call_back = nh.advertise<std_msgs::Int32>("/bizhang_callback", 1);
  ros::Rate rate(40);
  while (ros::ok())
  {
    ros::spinOnce();
    //ROS_INFO_STREAM("laser_control is: "<<laser_control.laser_control<<endl);
    rate.sleep();
  }

}